#ifndef ETL_FUNCTION_HPP
# define ETL_FUNCTION_HPP
# pragma once

#include <cmath>

#include <cstdint>

#include "simd.hpp"

namespace etl
{

#ifdef __SSE2__
template <typename E1, typename E2>
inline typename ::std::enable_if<
  (3 == E1::dimension) && (3 == E2::dimension),
  vector<double, 3>
>::type
cross(vector_expression<double, 3, E1> const& lhs,
  vector_expression<double, 3, E2> const& rhs) noexcept
{
  auto const r0(_mm_sub_pd(
      _mm_mul_pd(get_m128d<0, 1>(lhs), get_m128d<1, 2>(rhs)),
      _mm_mul_pd(get_m128d<1, 2>(lhs), get_m128d<0, 1>(rhs))
    )
  );

  auto const r1(_mm_mul_pd(__m128d{lhs(2), lhs(0)}, __m128d{rhs(0), rhs(2)}));

  return {r0[1], _mm_sub_pd(r1, _mm_shuffle_pd(r1, r1, 0x1))[0], r0[0]};
}
#endif // __SSE2__

#ifdef __SSE__
template <typename E1, typename E2>
inline typename ::std::enable_if<
  (3 == E1::dimension) && (3 == E2::dimension),
  vector<float, 3>
>::type
cross(vector_expression<float, 3, E1> const& lhs,
  vector_expression<float, 3, E2> const& rhs) noexcept
{
  auto const a(get_m128f<0, 1, 2>(lhs));
  auto const b(get_m128f<0, 1, 2>(rhs));

  auto const r(_mm_sub_ps(
      _mm_mul_ps(a, _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1))),
      _mm_mul_ps(b, _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1)))
    )
  );

  //r = _mm_shuffle_ps(r, r, _MM_SHUFFLE(3, 0, 2, 1 ));

  return {r[1], r[2], r[0]};
}
#endif // __SSE__

template <typename E1, typename E2>
inline constexpr typename ::std::enable_if<
  (3 == E1::dimension) && (3 == E2::dimension),
  vector<prod_type<typename E1::value_type,
    typename E2::value_type>, E1::dimension>
>::type
cross(vector_expression<typename E1::value_type,
  E1::dimension, E1> const& lhs,
  vector_expression<typename E2::value_type,
  E2::dimension, E2> const& rhs) noexcept
{
  return {
    lhs(1) * rhs(2) - lhs(2) * rhs(1),
    lhs(2) * rhs(0) - lhs(0) * rhs(2),
    lhs(0) * rhs(1) - lhs(1) * rhs(0)
  };
}

#if defined(__SSE__)
template <::std::size_t V, typename T>
inline typename ::std::enable_if<(0 == V), T>::type
sqrt(T const x) noexcept
{
  float r;

  _mm_store_ss(&r, _mm_rsqrt_ss(_mm_set_ss(x)));

  return x * r * (T(1.5) - T(.5) * x * r * r);
}
#elif defined(__ARM_NEON__)
template <::std::size_t V, typename T>
inline typename ::std::enable_if<(0 == V), T>::type
sqrt(T const x) noexcept
{
  auto const r(vrsqrte_f32({float32_t(x)}));

  return x * r[0] * (T(1.5) - T(.5) * x * r[0] * r[0]);
}
#else
template <::std::size_t V, typename T>
inline typename ::std::enable_if<(0 == V), T>::type
sqrt(T const x) noexcept
{
  constexpr ::std::int32_t const SQRT_MAGIC_F(0x5f3759df);

  float r(x);

  reinterpret_cast<::std::int32_t&>(r) = SQRT_MAGIC_F -
    (reinterpret_cast<::std::int32_t const&>(r) >> 1);

  return x * r * (1.5f - .5f * x * r * r);
}

template <::std::size_t V>
inline typename ::std::enable_if<(0 == V), double>::type
sqrt(double const x) noexcept
{
  constexpr ::std::int64_t const SQRT_MAGIC_F(0x5fe6eb50c7b537a9);

  auto r(x);

  reinterpret_cast<::std::int64_t&>(r) = SQRT_MAGIC_F -
    (reinterpret_cast<::std::int64_t const&>(r) >> 1);

  return x * r * (1.5 - .5 * x * r * r);
}
#endif

template <typename E>
inline constexpr typename E::value_type
length(vector_expression<typename E::value_type,
  E::dimension, E> const& e) noexcept
{
  return ::std::sqrt(dot(e, e));
}

template <typename E, ::std::size_t V = 0>
inline constexpr typename E::value_type
length(vector_expression<typename E::value_type,
  E::dimension, E> const& e) noexcept
{
  return sqrt<V>(dot(e, e));
}

template <typename E, ::std::size_t V = 0>
inline constexpr vector<typename E::value_type, E::dimension>
normalize(vector_expression<typename E::value_type,
  E::dimension, E> const& e) noexcept
{
  return e / length<V>(e);
}

namespace sincos_impl
{
  inline void sincos(float const v,
    float& first, float& second) noexcept
  {
    ::sincosf(v, &first, &second);
  }

  inline void sincos(double const v,
    double& first, double& second) noexcept
  {
    ::sincos(v, &first, &second);
  }

  inline void sincos(long double const v,
    long double& first, long double& second) noexcept
  {
    ::sincosl(v, &first, &second);
  }

  template<typename E1, ::std::size_t... Is>
  inline ::std::pair<
    vector<typename E1::value_type, E1::dimension>,
    vector<typename E1::value_type, E1::dimension>
  >
  get(vector_expression<typename E1::value_type,
    E1::dimension, E1> const& e, detail::indices<Is...> const) noexcept
  {
    decltype(get(e, detail::indices<Is...>())) r;

    ::std::initializer_list<int>{
      (
        sincos(e(Is), r.first(Is), r.second(Is)),
        0
      )...
    };

    return r;
  }
}

template <typename E1>
inline ::std::pair<
  vector<typename E1::value_type, E1::dimension>,
  vector<typename E1::value_type, E1::dimension>
>
sincos(vector_expression<typename E1::value_type,
  E1::dimension, E1> const& e) noexcept
{
  return sincos_impl::get(e, detail::make_indices<E1::dimension>());
}

}

#endif // ETL_FUNCTION_HPP
